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Abstract 

This paper provides a new observer design methodology for invariant systems whose state evolves on a Lie group with outputs in a 
collection of related homogeneous spaces and where the measurement of system input is corrupted by an unknown constant bias. 
The key contribution of the paper is to study the combined state and input bias estimation problem in the general setting of Lie 
groups, a question for which only case studies of specific Lie groups are currently available. We show that any candidate observer 
(with the same state space dimension as the observed system) results in non-autonomous error dynamics, except in the trivial case 
where the Lie-group is Abelian. This precludes the application of the standard non-linear observer design methodologies available 
in the literature and leads us to propose a new design methodology based on employing invariant cost functions and general gain 
mappings. We provide a rigorous and general stability analysis for the case where the underlying Lie group allows a faithful matrix 
representation. We demonstrate our theory in the example of rigid body pose estimation and show that the proposed approach 
unifies two competing pose observers published in prior literature. 
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1. Introduction 


The study of dynamical systems on Lie groups has been an 
active research area for the past decade. Work in this area is mo¬ 
tivated by applications in analytical mechanics, robotics and ge¬ 
ometric control for mechanical systems I0i]. Many mechani¬ 
cal systems carry a natural symmetry or invariance structure ex¬ 
pressed as invariance properties of their dynamical models un¬ 
der transformation by a symmetry group. For totally symmetric 
kinematic systems, the system can be lifted to an invariant sys¬ 
tem on the symmetry group 0. In most practical situations, 
obtaining a reliable measurement of the internal states of such 
physical systems directly is not possible and it is necessary to 
use a state observer. 

Systematic observer design methodologies for invariant sys¬ 
tems on Lie groups have been proposed that lead to strong sta- 
bili^ and robustness properties. Specifically, Bonnabel et al. 
ITHSt] consider observers which consist of a copy of the sys¬ 
tem and a correction term, along with a constructive method to 
find suitable symmetry-preserving correction terms. The con¬ 
struction utilizes the invariance of the system and the moving 
frame method, leading to local convergenc e prope rties of the 
observers. The authors propose methods in 1 13-12] to achieve 
almost globally convergent observers. A key aspect of the de¬ 
sign approach proposed in lfl3 - [l3] is the use of the invariance 
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properties of the system to ensure that the error dynamics are 
globally defined and are autonomous. This leads to a straight 
forward stability analysis and excellent performance in prac¬ 
tice. More recent extensions to early work in this area was 
the consideration of output measurements where a partial state 
measurement is generated by an action of the Lie group on a 
homogeneous output space j|3-13 II -Ill]. Design methodolo¬ 
gies exploiting symmetries and invariance of the system can be 
applied to many real world scenarios such as attitude estima¬ 
tor design on the Lie group SO(3) finins, pose estimation 


on the Lie group S E(3) ]I3-22]> homography estimation on the 
Lie group SL(3) ll23l] . and motion estimation of chained sys¬ 
tems on nilpotent Lie groups ilH] (e.g. front-wheel drive cars 
or kinematic cars with k trailers). 


All asymptotically stable observer designs for kinematic sys¬ 
tems on Lie groups depend on a measurement of system input. 
In practice, measurements of system input are often corrupted 
by an unknown bias that must be estimated and compensated to 
achieve good observer error performance. The specific cases of 
attitude estimation on SO(3) and pose estimation on SE(3) have 
been studied independently, and methods have been proposed 
for the concurrent estimation of state and input measurement 
bias ] 14, L7, These methods strongly depend on particu¬ 
lar properties of the specific Lie groups SO(3) or SE(3) and do 
not directly generalize to general Lie groups. To the authors’ 
knowledge, there is no existing work on combined state and 
input bias estimation for general classes of invariant systems. 


In this paper, we tackle the problem of observer design for 
general invariant systems on Lie groups with homogeneous out¬ 
puts when the measurement of system input is corrupted by an 
unknown constant bias. The observer is required to be imple- 
mentable based on available sensor measurements; the system 
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input in the Lie algebra, corrupted by an unknown bias, along 
with a collection of partial state measurements (i.e. outputs) 
that ensure observability of the state. For bias free input mea¬ 
surements, it is always possible to obtain autonomous dynam¬ 
ics for the standard error iMl, and previous observer design 
methodologies for systems on Lie groups rely on the auton¬ 
omy of the resulting error dynamics. However, for concurrent 
state and input measurement bias estimation, we show that any 
implementable candidate observer (with the same state space 
dimension as the observed system) yields non-autonomous er¬ 
ror dynamics unless the Lie group is Abelian (Theorem l4.1b . 
This result explains why the previous general observer design 
methodologies for the bias-free case do not apply and why the 
special cases considered in prior works 1120112 If do not naturally 
lead to a general theory. 

We go on to show that, despite the nonlinear and non- 
autonomous nature of the error dynamics, there is a natural 
choice of observer for which we can prove exponential sta¬ 
bility of the error dynamics (Theorems 15.11 and 15.31) . The ap¬ 
proach taken employs a general gain mapping applied to the 
differential of a cost function rather than the more restrictive 
gradient-like innovations used in prior work CMl. We also 
propose a systematic method for construction of invariant cost 
functions based on lifting costs defined on the homogeneous 
output spaces (Proposition 16. lb . To demonstrate the general¬ 
ity of the proposed approach we consider the problem of rigid 
body pose estimation using landmark measurements when the 
measurements of linear and angular velocity are corrupted by 
constant unknown biases. We show that for specific choices of 
gain mappings the resulting observer specializes to either the 
gradient-like observer of 1211] or the non-gradient pose estima¬ 
tor proposed in 1^ . unifying these two state-of-the-art appli¬ 
cation papers in a single framework that applies to any invariant 
kinematic system on a Lie-group. Stability of estimation error 
is proved for the case where the Lie group allows a faithful ma¬ 
trix representation. 

The paper is organized as follows. After briefly clarifying 
our notation in Section we formulate the problem in Section 
[3] A standard estimation error is defined and autonomy of the 
resulting error dynamics is investigated in section |4] We in¬ 
troduce the proposed observer in Section |5] and investigate the 
stability of observer error dynamics. Section |6] is devoted to 
the systematic construction of invariant cost functions. A de¬ 
tailed example in Section |7] and brief conclusions in Section [8] 
complete the paper. A preliminary version of this work was 
presented at the CDC 2013 lfl3l] . This manuscript was pub¬ 
lished in Automatica HI]. In addition to the material presented 
in 111], this paper contains detailed proof of theorems as well as 
detailed mathematical derivations of application examples. 


2. Notations and Definitions 

Let G be a finite-dimensional real connected Lie group with 
associated Lie algebra g. Denote the identity element of G by 
I. Left (resp. right) multiplication of X e G by 5 e G is 
denoted by LsX - SX (resp. RsX - XS). The Lie algebra 


g can be identified with the tangent space at the identity ele¬ 
ment of the Lie group, i.e. g = T/G. For any m € g, one 
can obtain a tangent vector at 5 e G by left (resp. right) 
translation of u denoted by 5 [m] TjLs[u] € TsG (resp. 
[m]5 ;= TiRs[u] £ TsG). The element inside the brackets [.] 
denotes the vector on which a linear mapping (here the tangent 
map TjLs : g —» TsG or TjRs : g —» TsG) acts. The adjoint 
map at the point 5 e G is denoted by Ad^ : g ^ g and is defined 
by Ads[u] := S[u]S-^ = [T/L^[«]] = TsRs-^ ° T,Ls[u] 

where o denotes the composition of two maps. For a finite- 
dimensional vector space V, we denote its corresponding dual 
and bidual vector spaces by V* and V** respectively. A linear 
map F: V* ^ V is called positive definite if v'*[F'[v*]] > 0 for 
all 0 V* 6 y. The dual of F is denoted by F*\V*^ V** and 
is defined by F*[v*] = v'* o F. The linear map F is called sym¬ 
metric (resp. anti-symmetric) if v*[F[w*]] = w*[F[v'*]] (resp. 
v*[F[w*]] = -w*[F[v*]]) for all v*,w* e V*, and it is called 
symmetric positive definite if it is symmetric and positive defi¬ 
nite. We can extend the above notion of symmetry and positive¬ 
ness to linear maps H : W —> VT* as well. Defining V := W*,H 
is called positive definite if H*: V* —> V is positive definite and 
it is called symmetric if H* is symmetric. Positive definite cost 
functions on manifolds are also used in the paper and should 
not be mistaken with positive definite linear maps. 

3. Problem Formulation 

We consider a class of left invariant systems on G given by 

X{t)^X{t)u{t), X{to) = Xo, (1) 

where m e g is the system input and X e G is the state. Al¬ 
though the ideas presented in this paper are based on the above 
left invariant dynamics, they can easily be modified for right 
invariant systems as was done for instance in Hi. We assume 
that u: —> g is continuous and hence a unique solution for 

([1]) exists for all t > to |3- In most kinematic mechanical sys¬ 
tems, u models the velocity of physical objects. Hence, it is 
reasonable to assume that u is bounded and continuous. 

Let Mi, i — !,...,« denote a collection of n homogeneous 
spaces of G, termed output spaces. Denote the outputs of sys¬ 
tem ([1]) by y, e M,. Suppose each output provides a partial 
measurement of X via 

yi^lii(X,yi) (2) 

where yi 6 M, is the constant (with respect to time) reference 
output associated with y, and /r, is a right action of G on M,, i.e. 
hi(I,yi) = y,- and hi(XS,yi) = hi(S,hi(X,yi)) for all y,- 6 M,- 
and all X,S 6 G. To simplify the notation, we define the 
combined output y (yi,... ,y„), the combined reference out¬ 
put y ;= (yi,... ,y„), and the combined right action h{X,y) 
{hi{X,yi ),..., h„{X,y„)). The combined output y belongs to the 
orbit of G acting on the product space Mi x M 2 x ... x M„ con¬ 
taining y, that is M {y e MiX M 2 x... x M„\y - h(X, y), X e 
G} c Ml X M 2 X ... X M„. Note that the combined action h 
of G defined above is transitive on M. Hence, M is a homoge¬ 
neous space of G while Mi x M 2 x ... x M„ is not necessarily 
a homogeneous space of G 1^ . 
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We assume that measurements of the system input are cor¬ 
rupted by a constant unknown additive bias. That is 


Uy - u + b 


(3) 


where «, e g is the measurement of u and 6 g is the unknown 
bias. In practice, bias is slowly time-varying but it is common 
to assume that b is constant for observer design and analysis. 

We investigate the observer design problem for concurrent 
estimation of X and b. The observer should be implementable 
based on sensor measurements. This is important since the ac¬ 
tual state X e G and the actual input « e g are not available for 
measurement and only the partial measurements yi,.. .,y„ and 
the biased input are directly measured. We consider the fol¬ 
lowing general class of implementable observers with the same 
state space dimension as the observed system. 

i = j(X,y,y,b,Uy,t) (4a) 

b = P(X, y, y, b, Uy, f) (4b) 


where X and b are the estimates of X and b, respectively, and 
y; GxMxMxgxgxR ^ TGandjS: GxMxMxgxgxR -> g 
are parameterized vector fields on G and g, respectively. Note 
that X, y, y, b, Uy and t are all available for implementation of the 
observer in practical scenarios. We refer to (l4al i and (l4bl i as the 
group estimator and the bias estimator, respectively. 


Example 3.1 (Pose Estimation on SE(3)). Estimating the po¬ 
sition and attitude of a rigid body has been investigate d by 
a range of authors during the past decades, see, e.g., 1791 - 


22l 1271/ . The full 6-DOF pose kinematics of a rigid body can 


be modeled as an invariant system on the special Euclidean 
group SE(3) 2^ 22l 2^/ . The Lie group SE(3) has a rep¬ 

resentation as a semi-direct product ofSO{3) and given by 
SE(3) = {(7?,p)| R e SO(3), p e R^} i2^/ . Consider the group 
multiplication on SE(3) given by R(s,g)(R,p) - L^fij,)(S,q) - 
(RS, p -h Rq) for any (R, p), (S, q) e SE(3). The identity element 
of SE{3) is represented by ( 73 x 3 , 03 ) and the inverse of an ele¬ 
ment (R,p) e SE(3) is given by (7?,/?)“* = (R^,—R^p). The 
Lie algebra of SE(3) is identified with se(3) = {(fl, y)| Q e 
so(3), V 6 R^} where so(3) denotes the Lie algebra of SO{3) 
represented as the set of skew-symmetric 3x3 matrices with 
zero trace. 

Let R be a rotation matrix corresponding to the rotation from 
the body-fixed frame to the inertial frame and suppose that p 
represents the position of the rigid body with respect to the iner¬ 
tial frame and expressed in the inertial frame. The left-invariant 
kinematics of a rigid body on SE(3) is formulated as 


{R, p) = T,L(r,p){LI, y) = {RLl, RV) (5) 

where Q resp. V represent the angular velocity resp. linear 
velocity of the rigid body with respect to the inertial frame ex¬ 
pressed in the body-fixed frame. Here, the group element is 
X — (R,p) e SE(3) and the system input is u — (Q, y) 6 se(3). 
Denote the measurement of the system input by (fly, Vy) £ se(3) 
and assume that it is corrupted by an unknown constant bias 
(bo), by) e se(3) such that (Q,., yy) = (Q - 1 - boj, V ■+■ by). Suppose 


that positions of n points with respect to the body-fixed frame 
are measured by on-board sensors and denote these measure¬ 
ments by yi,... ,y„ £ R^. Denote the positions of these points 
with respect to the inertial frame by yi, i = I,... ,n e and 
assume these positions are known and constant. The output 
model for such a set of measurements is given by 

yi ^ hi((R,p),yi) ^ R~^yi - R'^p, /=!,...,« (6) 

where hi is a right action of SE(3) on the homogeneous out¬ 
put space Mi R^. A practical example of measurements 
modeled by (0) is vision based landmark readings where the 
landmarks are fixed in the inertial frame, leading to constant 
ji, i = l,...,n The pose estimation problem is to esti¬ 

mate R and p together with the input biases 7>^ and by. □ 

4. Error Definition and Autonomy of Error Dynamics 

We consider the following right-invariant group error, 

E = £ G, (7) 

as was proposed in AH. The above error resembles the usual 
error x - x used in classical observer theory when x, x belong 
to a vector space. We have A = A if and only if £ = 7. We also 
consider the following bias estimation error 

b^b-beQ. ( 8 ) 

We are interested to see when an observer of the general form 
(IHi produces autonomous error dynamics since that would en¬ 
able straight-forward stability analysis. When the measurement 
of system input is bias free, implementable observers of the 
form (l4al i have been proposed that produce autonomous group 
error dynamics E llOl] . In this section, we show that when the 
measurement of system input is corrupted by bias, any imple¬ 
mentable observer of the form 0 produces non-autonomous 
error dynamics for general Lie groups, and it can only produce 
autonomous error dynamics for Abelian Lie groups. To prove 
this result, we note that the observer @ can be rewritten into 
the form 

A = X[uy -b]- a^(X, y, b, Uy,t), (9a) 

b - Py(X, y, b, Uy, t), (9b) 

where aj.: GxMxgxgxR ^ TGandySj,: GxMxgxgxR—>g 
are parameterized vector fields on G and g, respectively, and y 
is now interpreted as a parameter for and ySj. 

Theorem 4.1. Consider the observer (O/or the system 0-@. 
The error dynamics (E, b) is autonomous if and only if all of the 
following conditions hold; 

(a) ap and fii. do not depend on b, and t. 

(b) The vector field is right equivariant in the sense that 
T^RzapiX,y) — a^(XZ, h(Z, y)) for all X,Z e G and all 
y e M. 

(c) The vector field ySj, is right invariant in the sense that 
P^{X, y) = fy(XZ, h(Z, y))for all X,ZeG and all y e M. 
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(d) For all Z & G the adjoint map Adz- 0 —> 0 w the identity 
map. □ 

Proof: In view of ([1]) and (l9]l, differentiating E — XX ' and 
b -b — b with respect to time yields 

E - -Ty[Rx-ia^{X,y, b, Uy, t) - TiREAdyfi (10a) 

b - p^.{X,y, b, Uy, t). ( 10 b) 

If the conditions |(a)| to |(d)| of the Theorem hold, the error dy¬ 
namics will be simplified to 

E = -a^(XX-\hiX-\y)) - TiRsb = -a^(E, y) - TiReI), (11a) 
h^/3^(XX-\h(X-\y))^/3^.(E,y), ( 11 b) 

which are autonomous. 

Conversely, assume that the error dynamics (Unii are au¬ 
tonomous. Then there exist functions Fy. G x g —> TG and 
G X 0 ^ 0 such that for all X,X e G,y e M, b,Uy e 0 , 

E - —Tj^Rx-iaf(X,y,b,Uy,t) — TiREAd^b - F^{E,b) (12a) 

b = j3y(X,y, b, Uy, t) = //;.(£, b). (12b) 

It immediately follows that and are independent of Uy and 
t. Moreover, since the error E = XX ' is invariant with respect 
to the transformation (X, X) i-> (XZ, XZ) for all Z e G and the 
error b = b - b is invariant with respect to the transformation 
{b, b) (b + d,b + d) for all d e q,ws have 

- T^Rx-^ aiiX, y, b) - TjREAdfb = F^jE, b) (13a) 

= -7’xz^(xz)-iav(XZ, h{Z,y), b + d) - TjREAd^J), 
Py{X,y, b) = H^{E, b) = A.(XZ, h{Z,y), b + d). (13b) 

From (I13bb it follows that is independent of b since the right 
hand side of (I13bb depends on d while the left hand side is in¬ 
dependent of this variable. This establishes condition |(a)| for 
yS{.. It also follows that ySj, satisfies the invariance condition 
fi{X,y) - Pf{XZ,h{Z,y)) (condition |(c)| in the Theorem). We 
can rearrange (fiia to obtain 

-Tj^Rx-tap(X,y,b) - T'/T^gAd^h + TiREAd^^P (14) 

= -Txz^(xzy' OyiXZ, h{Z, y), b + d). 

The right hand side of (fT4b is a function of d while the left hand 
side is not. This implies that a,-, is independent of b (establishing 
condition |(a)| for a0. We can then rearrange (fT4b again to obtain 

-TxRx-'o;y(X,y) + 7’^2'^(xz)-iQ'v(XZ,/ 2 (Z,y)) (15) 

= TjREAdxb — TiREAdxx^. 

The right hand side of (fTSb is a linear function acting on 
h e 0 while the left hand side is completely independent 
of the variable b. Since b is arbitrary, this implies that 
both sides of (flST l are zero. In particular, Tj^Rx-ia^(X,y) = 
^xz^(xz)-> Q^.v(^Z,/!(Z, y)) and TjREAdj^J) - TjReA dj^b for 
all h e 0 and all E,X,Z e G. These equations imply 
T^Rzaf(X, y) = ^^(XZ, hiZ, y)) and Adzh - b to obtain con¬ 
ditions |(b)| and |(d)| imposed in the theorem, respectively. This 
completes the proof. ■ 


Remark 4.2. If G is a real, finite-dimensional, connected Lie 
group then condition \(d)\ of Theorem 14.71 implies that G is 
Abelian i2^ Proposition 1.91]. By the structure theorem for 
connected Abelian Lie groups Proposition III.6.4.11 ], this 

means that G is isomorphic to a product R" X (S')™ where R" is 
additive and (S*)™ denotes the m-dimensional torus. This class 
of Lie groups is far more specific than the Lie groups that are 
encountered in many practical applications. For robotics ap¬ 
plications, the Lie groups typically considered are SO(3) and 
SE(3) both of which are non-Abelian. Theorem \4.1\ in partic¬ 
ular implies that all implementable geometric bias estimators 
on SO(3) and SE(3) proposed in the literature produce non- 
autonomous standard error dynamics (see and Ml ). □ 

5. Observer Design and Analysis 

We propose the following implementable group estimator, 

X -X[uy-b] -X^(X,y,h, My, f)[Di0j;(X,y)], (16) 

with X(fo) = Xo where ff. G x M R^ is a cost func¬ 
tion, Di0j,(X,y) e TIG denotes the differential of with 
respect to its first argument evaluated at the point (X,y) and 
KfiX, y, b, Uy, t) is a linear gain mapping from TIG to T^G. 
Note that y is considered to be a parameter for X;, and fy The 
above group estimator matches the structure of (l9ab where the 
innovation is generated by applying the gain mapping Xj to 
the differential Di^ji. By Theorem 14.11 we already know that 
the above estimator cannot produce autonomous error dynam¬ 
ics for a general Lie group. Hence, there is no reason to omit 
the arguments b, m,. and t of the gain mapping. If the gain map¬ 
ping Xj, is symmetric positive definite and independent of b, m,. 
and t, the above group estimator simplifies to the gradient-like 
observers proposed in in for the bias free case, and in HI for 
the case including bias. 

We consider the following bias estimator, 

h^Y^T,L*^[Yli4>^{X,y)l h(fo) = ho, (17) 

where TjL*^: TIG 0 * is the dual of the map TjL^ (see Sec- 

tion| 2 ]) and F: 0 * —» 0 is a constant gain mapping. 

We will require the following assumptions for statement of 
results. 

(Al) Lie group G has a faithful representation as a finite¬ 
dimensional matrix Lie group. That is, there exist a posi¬ 
tive integer m and an injective Lie group homomorphism 
<h ; G ^ GL(m) into the group GL(m) of invertible mxm 
matrices. Note that 0(G) is a matrix subgroup of GL(m). 

(A2) [boundedness conditions] <h(X), <I)(X)“', u and 

K{X,y,b,Uy,t) are bounded along the system trajec¬ 
tories. 

(A3) [differentiability conditions] u(t), the first differentials of 
hfX, yi) and X(X, y, b, Uy, t), as well as the first and the sec¬ 
ond differentia^ of <p{X, y) with respect to all of their ar¬ 
guments are bounded along the system trajectories. 

* Second differential of is either in the sense of embedding the Lie group 
into R'"’*'" or in the sense of employing a Riemannian metric. 
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Theorem 5.1. Consider the observer ( Ii6l )-( I77I ) for the system 
0 - 0 . Suppose that assumptions \(A1 j| \(A2)\ and \(A3)\ hold. 
Assume moreover that the gain mappings K and F, and the cost 
function (p satisfy the following properties; 

(a) The gain mapping Kf. TIG T^G is uniformly positive 
definite (not necessarily symmetric). That is, there exist 
positive constants k and k and a continues vector norm 
|.| on T^G such that for all v* € T^G we have kfv*\^ < 

v*[Ki.{X,y,b,Uy,t)[v'‘]]<k\v*\^. 

(b) The gain mapping F; fl *—is symmetric positive definite. 

(c) The cost (p;. is right invariant, that is (pj{XZ,h{Z,y)) = 
(p{X,y)for all X,Z eG and ally e M. 

(d) The cost (pf(.,y): G —> E i—» (pp(E,y) is locally pos¬ 

itive definite around E — I and it has an isolated critical 
point at E — I. 

Then the error dynamics (E, b) is uniformly locally asymptoti¬ 
cally stable at (/, 0). □ 

Proof: The following result is used in the development later 
in this proof. 

Lemma 5.2. Let (py. G x M —> IR^ be a right-invariant cost 
function in the sense defined in part \(c)\ of Theorem \5.1\ Then 
we have 

Dpp^{X,y) = T^R*y^_AD,f^{E,y)] (18) 

Dy(p^{E,y) = Dicp^.(X,y) o TeRx (19) 

Proof of Lemma 15.2l is given in appendix. For simplicity, we 
denote K^.(X,y, b, Uy, t) by K^{.). Considering 0, 0, and (fThl l. 
the group error dynamics are given by 

E = tx^^ +XX-^ ^TgRx-^°T,L^[u\-T^Rx-^°T,L^[b] 

-TxRx-' °Ky{.)[Di(pi.{X,y)]-Tj(Rx-i oTjL^[u] 
^-T^Rx->oT,l^[b]-T^Rx-> oKp(.)oT^R;_, [Dif^(E,y)], ( 20 ) 

where E is as in 0 and (fTSl l is used in the last line of (l 20 l i. 
Now, consider the candidate Lyapunov function, 

£(E,b)^cp^(E,y) + ^Y-^[b][b]. (21) 

The Lyapunov candidate is at least locally positive dehnite due 
to conditions |(b)| and |(d)| The time derivative of X. is given by 

X(E,b) = Dif^{E,y)[E]+Y-^[hm. (22) 

Recalling that b - b and substituting E form (l20l) in (l22l i. we 
obtain 

t{E, h) = -Di^j(£,:y)[7>Rx-. °7i:j(.)or^R^_, [Di^j(£,y)]] 

-Xi,f^{E,y)[TxRx-^°TiLyim+Y-\hm- (23) 

Using (US, we conclude 

t(E,b)^-Xi,UE,mxRx-'°K^i-)°TgR]^_AXiif^(E,m 

-D,(P^iX,y)oTERxoT^Rx-^ o T:L^[b]+Y-^[hm 

= -YiiUE,y){TxRx-' oK^()oTy,R*y^_fYi,(p^(E,m 
- Di0^,y) o T,L^[b] + Y-^[B][b]. (24) 


Now, replacing b with (fTTl i we obtain 

j:{E,b)^-Dicp^{E,y)[T^Rx-^oKX.) o T^R*y,_dDpPXE,y)]] 

- D 1 (py(X,y) o TiL^ [b] + F^ 'o Fo F/Lt o D i f^X, y) [b]. (25) 

Duality implies Di0j,(X,y) o F/L^ = F/Ft oDi0,-.(X,y) and (l25T l 
simplihes to 

1{E, b) = -Xiicp^{E,y)[TxRx-'°m-)°TgR;^-^ [Di0,..(£,j;)]]. (26) 

Since 7Fy(.) is assumed to be positive dehnite and the map 
Tj^Rx-i is full rank, the map Tj^Rx-t o 7Fj,(.) o Tj^R*^_^ is pos¬ 
itive dehnite. This implies that X(E,b) < 0 and hence the 
Lyapunov function is non-increasing along the system trajec¬ 
tories. We adopt the proof of M Theorem 4.8] to prove uni¬ 
formly local stability of error dynamics. Recalling assump¬ 
tion |(A1)| distance to the identity element of G is denoted by 
d(.) and is induced by Frobenius norm on 0(G) c via 

d{E) ||Id - 0(£’)||f where Id is the identity matrix. De- 
hne the compound error x {E,b) e G x g and obtain the 
distance of x to (/, 0 ) by l(x)^ d(E)^ -H ||h||g where ||.||g 

denotes a norm on g. Using assumption |(d)| there exist a 
ball Br {E e G : d(E) < r] such that (py{.,y) is posi¬ 
tive dehnite on By. Consequently X{x) is positive dehnite on 
By [x & G X Q -. l{x) < r). Choose c < min;(jE)=r-C(^) and 
dehne Ylc ■- {x £ By\ X{x) < c). Since X.(t) < 0, any solu¬ 
tion starting in at to remains in for all t > to. On the 
other hand, since X{x) is positive dehnite on Qf c By, there 
exist class “K functions //^nd 772 such that p\{l{x)) < X{x) < 
t]2(1{x)) for all x e Od Lemma 4.3]. Consequently, we 
have l(x(t)) < p-\£(x(t))) < p(\X(x(to))) < p^HpiiKxito)))) 
which implies l{x{t)) < o r] 2 {l{x{to))). Since o 772 is a 
class 7C function (by Lemma 4.2]), the equilibrium point 
X - (/, 0) is uniformly stable for all initial conditions starting 
in Ylc i3d Lemma 4.5]. Moreover, the error E is bounded by 
d{E{t)) < l(x(t)) < (£Xx{to))) < 77]’*(c) for such initial con¬ 

ditions. 

Boundedness of x{t) implies that E{t) and b{t) are bounded 
with respect to d{.) and ||.||g, respectively. Differentiating 
(i26l l with respect to time and considering the boundedness of 
(E{t),b{t)) together with assumptions |(A2)| and |(A3)1 one can 
conclude that X(f) is bounded and hence J),(t) is uniformly 
continuous. By invoking Barbalat’s lemma we conclude that 
£.{t) —» 0. This together with condition |(a)| implies that 
Di<p^(E{t),y) —» 0. Since (p^(E,I) has an isolated critical point 
at £ = /, there exist a ball Bg (Z G such that £ = / is the 
only point in Be where Di<^j,(.,y) is zero. We proved before that 
E(t) e 'B.rho for all initial conditions starting in Q^. Choos¬ 
ing c < min( 771 (c), min;(jE)=r-£(^)) ensures that E - I is the only 
critical point in B,^-\(ey This implies that £(f) —» I for all initial 
conditions starting in Qf. Using O, (l9ali, and (|20ll, recalling 
assumptions |(A2)| and |(A3)| and using a local coordinate repre¬ 
sentation, one can verify that £(f) is bounded and hence £(f) is 
uniformly continuous. Thus, by invoking Barbalat’s lemma we 
have £(f) —> 0. Considering E{f), E(f) —> 0 together with error 
dynamics (i 20 l i implies that b{t) —> 0 for all initial errors starting 
in Qc. This completes the proof of uniformly local asymptotic 
stability of the error dynamics. ■ 
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The following theorem proposes additional conditions to 
guarantee local exponential stability of the error dynamics. 


of coordinates e L[[e]] and 6 L ^[[d]]. Using OTl i. the 
dynamics of the new error coordinates are obtained as 


Theorem 5.3. Consider the observer for the system 

dTJ-lO. Suppose that assumptions \(Al )\ and \(A2)\ and conditions 
\(a)\\(b)\ and \(c)\ ofTheorem \5.1\ hold. Assume moreover that; 

(d) Di^p(I,y) — 0 and Hess : g —> g* is symmetric 
positive definite. 

(e) The condition number of <t>(X{t)) is bounded for all t > to 
(uniformly in to). 

Then, the error dynamics {E{t), b(t)) is uniformly locally expo¬ 
nentially stable at (1, 0 ). □ 

Proof: The group error dynamics (l20l i can be rewritten as 

E = -T^Rx->oKi.(X,y,b,Uy,t)oT^R^_^ [Di0,•,(£', y)] - TiLEAA^b]. 

(27) 

Using (fTSl l and (fTTl i. the bias error dynamics is obtain as 

~b^T^T,Ll^T^R]^_AE)ify{E,y)] 

= roAd>r/L^[Di 0 |,(£,y)]. (28) 

Defining e,6 e g as the first order approximation of E and b 
respectively, linearizing the error dynamics (l27l i- (l28l l around 
(1, 0 ) and neglecting all terms of quadratic or higher order in 
(e, 6) yields 


e--TxRx-i oK;.{X,y,b,Uy,t)oTxRx-i oHessi0j(/,y)[e] 
-Ad;rfb], (29) 

b = roAdJoHessi(^j(/,y)[e], (30) 

where Hessi0j,(/,y): g —> g* denotes the Hessian operator 

which is intrinsically defined at the critical point of the cost 

iH. In order to investigate the stability of the linearized error 
dynamics, we consider a basis for the involved tangent spaces 
and rewrite (l29ll- (l30l l in matrix format. To this end, consider 
a basis {ey} for g and its corresponding dual basis for g*. Ob¬ 
tain the basis {ejX} for the vector space TxG by right translat¬ 
ing (ej) and consider its corresponding dual basis {(ejA)*} for 
T^G. Denote by [[e]], [[b]] the representation of the vectors e, b 
with respect to the basis {ej}. Denote the matrix representa¬ 
tion of the maps K^(X,y,b,Uy,t): T^G —> TxG, T: g* —» g, 
Hessi(^j,(/,y): g —> g* and Ad^: g —> g with respect to the 
above bases for their corresponding domain and co-domain by 
[[A]], [[T]], [[//]| and [[Adx]| respectively. Note that the matrix 
representation of Tj^Rx-i ; TxG —> g with respect to the cor¬ 
responding basis for its domain and co-domain is the identity 
matrix. Hence, the matrix representation of the error dynamics 
(EDI-® is obtained as 





-lAdxl 




ITMAdxFlT/l 

0 



Since T is symmetric positive definite, there exists a full rank 
square matrix L such that [[T]! = Lf L. Consider the change 


-LIKWHUL-^ -LlAdxW 
LIAdxFlT/ll-L-i 0 


(32) 


Consider initial conditions A(fo) for system ([T]i and (A(fo), Kto)) 
for the estimator (fT6l l- (fT7l i. respectively. Introducing the param¬ 
eter A = (to, X(to), X(to), b(to)) £ E) where D:=][lxGxGxg, 
the trajectories of X,X,b and y can be viewed as functions 
of t and A. Define A(t,A) -L[[A]][[//]|L“', B(t,A) 
-L[[Adx]|^T^, and P The system (l32l i belongs 

to the following standard class of parameterized linear time- 
varying systems discussed extensively in the literature 0321 - 1341] . 


Ait, d) 
-B(t, A)P 


Bit, AY 
0 


(33) 


We can now verify the conditions of 034l Theorem 1] to prove 
the stability of system (l32l i. Both Bf, d) and its time deriva¬ 
tive are bounded due to Assumption |(A2)| Since Hessi0,-.(/,y) 
is symmetric positive definite and L has full rank, the ma¬ 
trix P is symmetric positive definite and it is bounded by 
ofH)ofLY^I < P < &iH)d-iLY^I where of .) and (t(.) denote 
the smallest and largest singular value of a matrix respectively. 
Define -Q P + Ait, A)'^P + PAf, A) = -(IH]]L-')'"(|[AF + 
I^]1)(I7/]]L“'). Using condition |(a)| of Theorem 15.1! and re¬ 
calling assumption |(A2)| there exist positive constants ki and 
k 2 such that 2kild < [[A]] < 2 ^ 2 Id where Id is 

the identity matrix. This ensures that Q is uniformly sym¬ 
metric positive definite and we have 2kiofH)^ofL)~Yd < 
Q < 2&iHfk2a-iL)^Yd. It only remains to investigate 
whether B(f, d) is d-uniformly persistently exciting ifj^. equa¬ 
tion (10)]. Embed the Lie algebra g into Invoking the 

property vec(<l)(A)w<I>(A)“') = <I)(A)“'^ ® <l)(A)vec(w) where 
vec(w) e E™ is the vectorization of the matrix w e g and 
0 denotes the Kronecker product, one can conclude that the 
matrix representation of Adx : E"'^"' ^ E'"^'" with re¬ 
spect to the standard basis for its domain and co-domain is 
given by [[Adxl] = <I)(A)-'" ® <[)(A). Thus o:(IAdxI]) = 
^(<h(A)“'^)o^(<l)(A)) = cond(<l)(A))“' where cond(<l)(A)) de¬ 
notes the condition number of 0(A) e GL(m). Since g c E”'^™, 
the minimum singular value of Adx : 0 —> 0 is larger than 
or equal to the minimum singular value of Adx : E™^'” —» 
E^x™. Using condition |(e)| there exists a positive constant 
Co such that cond(0(A)(f)) < co. Hence, ofBit, A)Bit, d)^) = 
^(LIAdxFirMAdxlL^) > aiL)Min)c-'^ co. Integrat- 

cf'fT 

ing both sides yields Bjr, A)BiT, A)^dT > coTId which 
completes the requirements of 1134 Theorem 1]. Hence, the 
equilibrium (0,0) of the (l3^ is uniformly exponentially stable. 
This implies that the equilibrium (0,0) of the linearized sys¬ 
tem (l32l i is uniformly exponentially stable and consequently the 
equilibrium (/, 0) of the nonlinear error dynamics (l27T i- (l28l l is 
uniformly locally exponentially stable Theorem4.15] (note 
that what is referred to as uniform exponential stability here is 
the same as exponential stability in the sense of S). 

Owing to the parameter-dependent analysis, the obtained ex¬ 
ponential stability is uniform with respect to the choice of all 
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initial conditions in A and not only with respect to the choice of 
^(fo) and fo(fo) for a given X. ■ 

Remark 5.4. For the stability analysis, we assume that G al¬ 
lows a matrix Lie group representation (by assumption \(A1 jf . 
Nevertheless, the actual formulas of the proposed observer 
(ESll-lO can be computed without requiring any matrix struc¬ 
ture for the Lie group, owing to the representation-free formu¬ 
lation of the proposed observer. We only require the matrix Lie 
group representation of G to interpret the boundedness condi¬ 
tions on 0(X), <I>(X“*), and cond(^(X)). We will illustrate this 
point further with an example in section [2 Boundedness of 
<t(X(f)) and <I>(X“*(f)) are usually mild conditions in practice. 
Moreover, it is easy to verify that cond{X{t)) is bounded (uni¬ 
formly in to) if(A>(X(t)) and 0(X“*(f)) are bounded (uniformly 
in to). For the special case where the considered Lie group 
is S0(3), all of these boundedness conditions are satisfied au¬ 
tomatically since we have ||<5(.X’(f))llf = fr(<[>(X)^<l)(X)) = 
tr{fx 3 ) — 3 for all X e S0(3). In section^ we interpret the 
boundedness requirements for the Lie group SE{3) as well. □ 

It is possible to replace the requirement for boundedness 
of <l)(X(f)), and cond(<l)(X(f))) respectively with the 

boundedness of <l)(l(f)), 0(X-*(f)), and cond(<D(X(f))) in The¬ 
orems I5T] and 15.31 and still prove the same stability results. 
Boundedness conditions on X are always verifiable in practice. 

Theorem O does not necessarily require a symmetric gain 
mapping K^.. Also, we do not impose any invariance condi¬ 
tion on this gain mapping. Condition |(a)| of Theorem ISTI only 
requires the symmetric part of Ki, denoted by Kf to be uni- 
formly positive definite. Considering a basis for Tj^G and the 
corresponding dual basis for TIG, condition |(a)| of Theorem lSTI 
implies that the matrix representation of Kf(.): TIG —> T^G 
with respect to these bases is uniformly symmetric positive def¬ 
inite. In practice, we will use this property to design a suitable 
gain mapping and obtain the innovation term of the observer. 
We will illustrate this method with an example in Section|2l 

Condition 1(d) I of Theorem lSTI is milder than condition |(d)|o f 
Theorem 15. 3 1 or similar conditions imposed in ifioll and lll3ll . 
This allows the choice of much larger class of cost functions to 
generate innovation terms that guarantee the asymptotic stabil¬ 
ity of error dynamics. 

In the special case where K^. is uniformly symmetric posi¬ 
tive definite and is independent of the arguments b, Uy and f, 
the term K^(X,y){L)\f^(X,y)'\ simplifies to gmdif^(X,y) where 
gradj denotes the gradient with respect to the Riemannian met¬ 
ric on G induced by the gain mapping. In this case, the ob¬ 
server (fTbl-lfTTl i simplifies to the gradient-like observer dis¬ 
cussed in 111 equations (7)-(^ where the gain mapping T is 
a scalar, or the observer of llOll for the bias-free case. If in 
addition we assume that K^. satisfies the invariance condition 
T^Rz o KiiX, y)oT- K^(XZ, h(Z, y)), the induced Rieman¬ 
nian metric on G would be right-invariant. In this case, the error 
dynamics (l27T i-(l28ll correspond to the perturbed gradient-like 
error dynamics given by il3[ equations (17)-(18)]. The larger 
class of gain mappings together with the larger class of cost 
functions proposed in this paper ensures that the proposed ob¬ 
server allows a much larger class of observers comparing to the 


authors’ previous work 1 1112, 13]. The discussion presented 
here shows also that a non-invariant Riemannian metric can be 
employed for the bias-free case to design the innovation term of 
the gradient-like observers in M\ T2LI13I] . In this case, the re¬ 
sulting error dynamics would be stable as long as the conditions 
on the cost function are satisfied, but the error dynamics would 
be non-autonomous. Non-invariant gains also lead to observers 
that are not symmetry-preserving in the sense of jjst]. 


6. Constructing Invariant Cost Functions on Lie Groups 

In Section |5l we propose the observer (fT6l l- (fT7l i that depend 
on the differential of the cost function fy. G x M —> as its 
innovation term. The cost function must be right invariant, 
and it should satisfy condition |(d)| of Theorem l5.1l (or condition 
|(d)| of Theorem l5.3b in order to guarantee asymptotic (or expo¬ 
nential) stability of the observer error. Designing such a cost 
function can be challenging since M is an orbit in the product 
of different output spaces which can generally be a complicated 
manifold. In this section, based on the idea presented in |@], we 
propose a method for constructing a suitable cost function by 
employing single variable cost functions on the homogeneous 
output spaces M,. Finding a suitable cost function on each out¬ 
put space is usually easy, especially when the output spaces are 
embedded in Euclidean spaces. 

Proposition 6.1. (0/ Suppose fly. Mi R^, y, i-> fl fyO are 
single variable cost functions on Mi, i — Corre¬ 

sponding to each fl, construct a cost function 01 : G X Mi —» 
R^ using (pi (X,yi) /.' (/z,(X“*,y,)). Obtain the cost function 

(a) The cost function 0j, is right invariant in the sense defined 
in part \(c)\ ofTheorem \5.1\ 

(b) Assume that each fl, i = l,...,n is locally posi¬ 

tive definite around % e M;. Assume moreover that 
H/Li stabhiiyd — {/) where stab^fyi) denotes the stabilizer 
ofyi with respect to the action hi, defined by stabhfyi) 

{X e G : hi(X,yi) = y,). Then 0j,(.,y) : G —> R^ is locally 
positive definite around I eG. 

(c) IfDfilji) - 0 for all i = !,...,« then D\(p^{I,y) - 0. If 

additionally the Hessian operators Hessfl {%)'. T^Mi —» 
T* Mi are symmetric positive definite for all i — !,...,« 
and P|"_j Tistabhfyi) — {0}, then Hess\(p\.(I,y) is symmet¬ 
ric positive definite. □ 

Proof of Proposition |6T| is given in the Appendix. Proposi¬ 
tion |6T| suggests a systematic method to construct a cost func¬ 
tion which satisfies the requirements of Theorem 15.II or Theo¬ 
rem 1531 The differential of this function can be employed to 
design the innovation term of the observer. We will illustrate 
this method with an example in section|2] 

The method proposed by Proposition ib.ll to construct the cost 
function 0 ,-, is basically different from the one presented in E 
Proposition 2]. The method proposed in E employs invari¬ 
ant cost functions on M, x M, while the method presented here 
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only requires single variable cost functions on each M,. Imple- 
mentability of the proposed observer in 11311 is guaranteed when 
the homogeneous output spaces are reductive. The method pre¬ 
sented in this paper guarantees the implementability of resulting 
observer without imposing any reductivity condition. 

The condition n"=i stab^^lj,) = {/} (imposed in part |(b)| of 
Proposition 16.1b is sufficient to ensure n"=i T/Stab;,,!^,) = {0} 
(imposed in part |(c)| of the Proposition). This condition can be 
interpreted as an observability criterion. In particular, for the 
attitude estimation problem with vectorial measurements, this 
condition is equivalent to the availability of two or more non- 
collinear reference vectors 1131]. As will be discussed in the 
next section, for the pose estimation problem with landmark 
measurements, this condition corresponds to the availability of 
three or more landmarks which are not located on the same line. 

The method presented in this paper suits the systems with 
constant reference outputs. Time varying reference outputs 
have been investigated in |15, T§, 3 36] for attitude estima¬ 
tion problem on SO(3). Nevertheless, in most practical cases, 
the reference outputs are approximately constant Cl 130 
and the proposed observer design methodology applies. 

7. Example: Pose Estimation Using Biased Velocity Mea¬ 
surements 

Recalling the pose estimation problem discussed in Example 
13.11 here we employ our observer ( T6Tl- (fT7l i to derive the pose 
estimators proposed in ] 21 ] and ] and we generalize them. 

Apart from the semi-direct product representation of SE(3) 
discussed in Example 13. II it is known that SE(3) has also a ma¬ 
trix Lie group representation as a subgroup of GL(4) (see e.g. 
12111'). We use this matrix Lie group representation only to in¬ 
terpret the required boundedness conditions (see Assumption 
l(A2)| i but we employ the semi-direct product representation to 
derive the observer formulas (see remark l54l i. The Lie group 
homomorphism O which maps an element {R, p) e SE(3) to its 
corresponding matrix representation in GL(4) is given by; <t> : 
R p 


(R,P) 


0 1 


. The Lrobenius norm of <^{{R,p)) e GL(4) 


is given by ll<l)((R,p))lP = tr((l>((R, p))^<l)((R, p))) = 4 + \\p\\\ 
Hence, <l)((R(f),p(f))) is bounded if p{f) is bounded. Similarly, 
one can verify that <l)((R(f),p(f)))"' and cond((R(f),p(f))) are 
bounded (uniformly in to) if p{f) is bounded (uniformly in to). 
This characterizes the boundedness conditions imposed by As¬ 
sumption [(A^ and part ](e)] of Theorem l5.3l 

Lrom here after, we only consider the semi-direct product 
representation of SE(3) ^ SO(3) x E,^. We aim to employ 
the observer developed in section |5] and use the guidelines pre¬ 
sented in section | 6 ] to design an observer to estimate the pose 
X - {R, p) and the bias b - {bu, by). Let us first evaluate the ob¬ 
servability condition imposed by part](b)]and](c)]of Proposition 


M We have nti stab/,(y,) = {{R,p) e SE(3): R^yi - R^p = 
y,■,/=!,...,«} = {(R,p) e SE(3) : R~^p = -y;, R(y; - 

yj) - yi ~ yj j ~ ■ ■ ■ > i + i) which implies that y; - yj 

is an eigenvector of R. Hence, a necessary and sufficient con¬ 
dition which guarantees P|"^j stab/,,(y,) = {(/ 3 x 3 , 03 )} (and con¬ 
sequently Pl/Li T(/^ 0 )StabA,(y;) = {( 03 x 3 , 03 )}) is the existence of 


at least three reference outputs %,yj,% such that y, - yj is not 
parallel to yj - Note that this condition is independent of 
the choice of inertial frame. Specifically, when landmark mea¬ 
surements are employed to provide outputs y,-, / = this 

condition is equivalent to the existence of at least th ree land¬ 
marks which are not located on the same line | 


m- 


In order to design the innovation terms of the estimator (fTbl l- 
CIl), we resort to choose a basis for each tangent space to obtain 
matrix representations for the linear mappings Kj,, T, TiL\ and 
use simple matrix calculus. Lor the sake of clarity, we denote 
the matrix representation of a linear mapping F: U —> W with 
respect to the basis {u) for its domain and basis (w) for its co¬ 
domain by the notation ][L]]*. Also, the R" representation of a 
vector a e U with respect to the basis {u) is denoted by ][a]]u. 
Denote the standard bases of R^ and so(3) by {e) and (ex), re¬ 
spectively. Using these bases, one can obtain a standard basis 
for se(3) denoted by (e). We obtain a basis for 7 ’(Ap)SE( 3) using 
the right translation of {e}. Denote this basis of 7'(Ap)SE(3)by 

(eX) and its corresponding dual basis of 7”*. SE(3) by {(eX)*}. 

_ (*./') 

In order to use Proposition 16.11 we start by designing suit- 
R^. A simple cost function is constructed 

- y,]]^, kj > 0 where ]].]] denotes the Eu¬ 
clidean distance. It is straight forward to verify that /.' satisfies 


able costs // ; M, - 

by Kiyi) |lly/ 


the requirements imposed by part ](c)] of Proposition 16.11 i.e. 


D/|(y,) = 0 and Hess/1 (y,) is symmetric positive definite. The 
cost functions: SE(3)xM, —> R^, i - 1,...,« are obtained 


as <f>'.,iX,yi) 


_ ki 


2 ||/2,((R,/) ',y,) - y,]]^ = jWRyi + p - y,l 


Denoting an arbitrary element of se(3) by (O, V), we have 
V)] = (ClR,Clp + V) 6 7’(^,;,)SE(3). One can 
obtain Di/j((R,/),y): 7’(^^)SE(3) ^ R as 

Dicf>^({RSm^^^P+V)]=J^l^kiaJ(Myi+np+V). (34) 

where a, := (Ry, + p - yd e R^. The R® representation of 
Di/j(X,y) e r^SE(3) is the transpose of the matrix represen¬ 
tation of Di/ 5 ,(X,y) : 7’^SE(3) ^ R, i.e. IDi/j(X,y)](^^). = 

(][Di/j,(X,y)]]^^) . Employing (l34l) and using the simplifica¬ 
tions given in the Appendix, we obtain 

]IDi/5.(X,y)]]b = Y^''.^^ki[yJ(Ry + p)x,aJ] (35) 

We choose lKj,(X,y,b,Uy,t)Y^j^^. = diag(k^/ 3 x 3 , ^ 3 x 3 ) where 
ku,ky are positive scalars and ensure that the resulting gain 
mapping Kj.(X,y,b,Uy,t): T\SE{3) 7’(^^)SE(3) is uni¬ 


formly positive definite. Using (I35b we have 




- (Z"=i f 

where the argument (X,y,b,Uy,t) of Kj, has been omitted for 
brevity. We use (1451) of Lemma 18.11 given in the Appendix to 
obtain 

7:j(.)[Di/,.,(X,y)] (37) 

= {-kai((Ryi + p)xyi)xR, -kMRyi + p)xyi)xp+kya^ 
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Choosing the gain [[r]]!. = diag(y^/ 3 x 3 , Tv'^axa), we have 

iro7}L![Di,^j,(i;y)]]|e - ml 

JuhxS 03x3 
03x3 yvhx3 

yajjixi^lh - R^P) 

yyR^ai 

where the term has been computed in the Ap¬ 

pendix. One can employ (H4l i of Lemma ISTI given in the Ap¬ 
pendix to obtain 

ro7>Z^[D,0^(je,y)] ^.Ji{y^(yUR^h-R^p))x, (39) 
Using (iJTl i and ( l39l l, the observer is summarized as 



R 03x3 

1 

PxR R 



■ 2?=i ki(Ryi + p)xh 

i:U km 


(38) 


k = R{D.y - bj + ^ ki{{Ry + p)xh)xk (40a) 

n 

p^R(Vy-b,.)+Y^ki(ka,((Ry + p)xh)xP-ky{Ryi+p-%)) (40b) 

/-I 

bw = yoj ^ ki(yi^{R'^% - R'^p))x (40c) 

by = yy{R~^p - R^h + yd (40d) 


Notice that the resulting observer formulas (I40al i- (l40db do not 
depend on the chosen basis. Omitting the bias estimator, the 
group estimator (I40ab - (l40bb h as a similar form as the gradient¬ 
like observer proposed in ml equation (35)] since the chosen 
gain mapping K^. is symmetric positive definite and yields a gra¬ 
dient innovation term. 

The pose estimator of has a different form from (l40b . 
Here, we derive the observer of Q by choosing different gain 
mappings and output maps. Similar to lEol equation ( 8 )], con¬ 
sider the new set of outputs zj, j - given by 


Zn - 


Z n-l 

aij(yi+i - yd, 

1 




n Z-ii= 


yi- 


(41a) 

(41b) 


We employ the cost functions //(zy) ylkj - zylP, kj > 0 
and we choose the gain mappings ^Ki{X,y,b,Uy,t)Y-~ , - 

{eXy 

diag(k„,/ 3 x 3 ,k,/ 3 x 3 ) + diag( 03 x 3 , (^(f^v - bj)^) and [[ 0 ^ = 
diag(yfj/ 3 x 3 , yyhxi)- It is easy to verify that this choice of cost 
functions and gain mappings satisfies the requirements of our 
method. Notice that is non-symmetric and depends also on 
Qy and bo) unlike the previous part. In particular, this implies 
that the observer innovation is not a gradient innovation. Nev¬ 
ertheless, the symmetric part of /Q is diag(k„/ 3 x 3 , kylj,x 3 ) which 
implies that the resulting gain mapping is uniformly positive 
definite. Following the same procedure as was done to derive 
(Eil, we obtain the following observer. 

R^R(^ly-bJ-k^k„(pJ:dxk+k^ Y^.^yjk((R^z)xz)^ (43a) 

p^R(Vy-bd+k,i{kyhy,i + (R{^ly-bJ)y,)(Rz„ - p-Zn) 

- ky,kn{pxZn)xP + ka> kj{{RZj)xZj)y^P (43b) 

bw^yojk„[R^Px(Rz„-p))^ + yoj kj{(R'^Zj)xZj)^ (43c) 

k: = -yyk„R'^(Rz„ -p-Zrd (43d) 

In ll^ . it is assumed that the origin of inertial frame is lo¬ 
cated at the geometric center of the landmarks. In this case 
we have z„ - 0 which simplifies the observer (l43l l to the ob¬ 
server designed in ll 2 (iP[ Compared to ll^ . the observer (l43T l 
has the advantage that it is well-defined even if only some of 
the measurements y, are unavailable at some period of time. 
In this case, the reference output z„ can be recalculated us¬ 
ing the reference outputs corresponding to the remaining avail¬ 
able measurements. Also, we only require A - [a,^] to be full 
rank but lEoll necessarily requires that are chosen such that 
[zi, ... ,z„-i][zi, .. .,Zn-\Y - hx 3 - For practical implementa¬ 
tion purpose, discrete time representation of the observers could 
be obtained usin g sta ndard structure preserving numerical inte¬ 
gration methods 13711 . 


8. Conclusion 


We assume that e R are such that the matrix A [a,y] e 
R'”-'>x(«-i) 

is full rank. This requirement guarantees that no 
information is lost by applying the linear transformation (l4Tb 
to the measurements. Substituting y, from ® into (l4Tb and 
defining new reference outputs zy TIYi‘^ijYi+y ~ h)^ j - 
1,..., n - 1, z„ = Yj1=i h yields 

Zj ^ 8j((R,p),°Zj) R^Zj, 7 = (42a) 

z„ = gn{{R, p), Zn) ;= R^°Zn + R^P (42b) 

where gj, j - !,...,« are right output actions of G. Consider 
the new combined output z (zi,. .. ,Zn) and the combined 
reference output z (zi,... ,z„). One can show that the nec¬ 
essary and sufficient condition for n"=i stabg^.(zy) = {/} is the 
existence of at least two non-collinear reference outputs zj,Zk- 
Assuming that A = [a/y] is invertible, it is straight forward to 
show that the above mentioned condition on z is equivalent to 
the condition on y we derived before. 


We investigate the problem of observer design for invariant 
systems on finite-dimensional real connected Lie groups where 
the measurement of system input is corrupted by an unknown 
constant bias. We show that the corresponding standard error 
dynamics are non autonomous in general. We propose an ob¬ 
server design methodology that guarantees the uniform local 
asymptotic (or exponential) convergence of the observer trajec¬ 
tories to the system trajectories. We employ a gain mapping act¬ 
ing on the differential of a cost function to design the innovation 
term of the group estimator. The bias estimator is then designed 
using a Lyapunov method. The notion of homogeneous out¬ 
put spaces is generalized to multiple outputs, each of which is 
modeled via a right action of the Lie group on an output space. 


^Here, the position vector p is expressed in the inertial frame but in da 
the position vector is expressed in the body-fixed frame. One can transform the 
system of (20h to the form presented here using the change of variable p Rp. 
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A systematic method for constructing invariant cost functions 
on Lie groups is proposed, yielding implementable innovation 
terms for the observer. A verifiable condition on the stabilizer 
of the reference outputs associated with the output spaces en¬ 
sures the stability of the observer. This condition is consistent 
with the observability criterion discussed in . Our proposed 

method omits the limiting reductivity condition imposed in the 
authors’ previous work |T2, 13]. As a case study, pose esti¬ 
mation on the Lie group SE(3) was investigated where our ob¬ 
server design methodology unifies the state-of-the-art pose es¬ 
timators of and lEl] into a single framework that applies to 
any invariant kinematic system on a Lie-group. Extension of the 
proposed observer design methodology to the (co)tangent bun¬ 
dle of a Lie group could be considered by assigning a Lie group 
structure to the (co)tangent bundle noting that the (co)tangent 
bundle is trivial (see e.g. I^ L 


APPENDIX 

Proof of Lemma 15.21 

The right-invariance property of implies (py{X, y) - o 
Rx-i(X,y). Differentiating both sides in an arbitrary direction 
V e Tj^G and using the chain rule we obtain Di(^j,(A,y)[v] = 
Di 0 v(£',y) o T^Rx-i{v\. Since v is arbitrary and by using the 
duality we have Di 0 ,-.(X,y) = [Di<^j,(£’,y)] which proves 

(ITSl l. Applying (Tj^Rx-iy^ - TeRx from the right to both sides 
of Di 0 ,-,(A,y) = Di<pp(E,y) o Tj^Rx-t yields ( fT9l l. ■ 


Proof of Proposition 16.71 
Part \(a}\ 

For any arbitrary Z e G have (f>^(XZ,h(Z,y)) - 

i:ti 4 (/h((^Z)-‘,/h(Z,y))) = i:Uf;MZZ-^X-\y)) = 

2"_i flfhfX^^,y)) - (pp(X,y) which shows that 0,-. is right in¬ 
variant. 

Part \(b)\ 

Since fifyd is positive definite around y, = y,-, there ex¬ 
ists a neighborhood A, c M,- of y,- such that fi^yd ^ 0 and 
(y,) = 0 ^ y; = y, for all y,- e A,. Corresponding 

to each A,, define the set Ni {E E G : hi(E^^,yi) E 
A} c G and consider the set A := HJLiIt is easy 
to verify that A c G is a neighborhood of I and we have 
(py(E,y) = ^ 0 for all E E N. Moreover, 

for any E E N, (f>^{E,y) = = 0 yields 

f^flifE^^ ,yi)) - 0 for all i — This in turn implies 

that hi{E~^,yi) i - . ,n and hence E E H/Li stab/,ly,). 

We assumed P|"_j stabA,(y;) = {/} which ensures that E - I and 
hence <py(E, y) is positive definite on A. 

Part\(c)\ 

Define the map h^..: G —» M, by h^X : = 
hi{X,yi). Differentiating both sides of (p^{E,y) - 

2"_i(/i,(£^',y,) in an arbitrary direction v E TeG 
and using the chain rule we have Di 0 ,-.(£,y)[v] = 


- 2"=i E>f^,(h(E \y,)) o Te-iIi^i o T,Le-> o TeRe-> [v]. Evaluat¬ 
ing the later relation at E = I and omitting the arbitrary argu¬ 
ment V we obtain Di(^,”.(/,y) = - 2/Li D/^ CV;) ° T’/h,-.;. Hence, 
D/7 (y,) - 0, i - 1,...,« implies Di0j,(/,y) = 0. Under this 
condition, standard computations shows that Hess i/>{.(/, y) = 
2 :;LiHessi,^,(/,y,) = Sti 7’/^, ° ^ess/^'.Cv,) o T/A, where 

Till*. : T*^.Mi —» TjG denotes the dual of Tih^... If all of 
Hess/.'(y,), i - !,...,« are symmetric positive definite, 

then Hessi/j,(/,y) is symmetric positive semi definite with 
ker(Hessi/j,(/,y)) = PlJLi ker(7’/lAi)' Since, ker( 7 ’// 2 j/ = 
T/Stabft/y,), we have flJLi ker(r//zj,) = n"=i T/Stab/j/y,) which 
is assumed to be (0). Consequently, ker(Hessi/j,(7,y)) = {0} 
which implies that Hess i/;.(/,/) is full rank and hence 
symmetric positive definite. ■ 

Computing [[Di/,".(A,y)]]l^ employed in m: 

The standard basis for is given by {e} {e/e^,e^}. 

The standard basis for so(3) is obtained as {ex} 
{ex,ex,ex} and a basis for se(3) is represented by je) 

((ei, O 3 ), (e2, 03 ), (e^, O 3 ), ( 03 x 3 , e'), ( 03 x 3 , e^), ( 03 x 3 , e^))- A ba¬ 
sis for 7’(^^)SE(3) is obtained by right-translating the basis of 
se(3) as 


(eX) ;= ((e^^, e^p), (Cx^, e^p), (e^A e^p), 

(03x3, e'), (03x3, e"), (03x3, e^)). 

We employ the above basis to obtain 


IDi/,..(X,y)]] 


eX 


/-I 

n n 


/=i 


i=l 


Lemma 8.1. Suppose that the R® representation of u E se(3) 
and w E 7’(^^)SE(3) with respect to the basis {e} and {eX} are 
respectively given by fuje - [m^ 

where m^, iq,, w^, Wy E R^. Then u and w can be written as in 
terms of their R® representation as follows. 

u^{Uojx,Uv) (44) 

W = (WtyxE, w^xP + v^v)- (45) 

□ 


Proof: 

w ^wl&\&iR, q\p) + wle^itlR, &lp) + wler’icl.R, e^p) 
H-wje'( 03 x 3 , e') H-wje^( 03 x 3 , e^) -H wje^( 03 x 3 , e^) 


1 


.,T^ 2^2 


=((w,:,e‘e; H- w^e'ex -H w^e q^)R, 


<(v 

' e -H w^e^e^ -H w^e^e^)/ -H w;[e * e' -H wJe V -H w;^e^e^) 
^(WyxR, WvxP + W,) 
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where we used the standard equation a - a^e'e' + a^e^e^ + 
a^e^e^ once for a - Wa, and once for a = w,, to obtain the last 
line. This proves (l 45 l l. Choosing {R,p) = (/3x3,0), it is easy to 
verify that (l 44 l i holds too. ■ 


Computing employed in 03 .' 

Suppose [a^,b^]~^ e IR® as the first column of 
and denote by a,, b,, / = 1 ,... 3 the elements of a, b 6 R^. We 
have, 7 ’/L^[(ei, 0 )] = (^6^,0) = aii^xR^fxP) + bi( 0 ,e‘) = 
{aie'y^R,afi'^p + bjC). This implies that Rel, - ajO'^R 
and 0 = which together form 6 linear equa¬ 

tions with 6 unknowns. Solving this set of equations yields 
a = ^e' and b - px^e'. Consequently, the first column 
of is given by [(.Re')^, (px^e')^]^. One can use 

the same procedure as was explained above to obtain the sec¬ 
ond and third column of as [(^e^)"^, (px^e^)^]^ 

and [(.Re^)^, (px^e^)^]^ respectively. Suppose [c^,c/^]^ as 
the forth column of [[7’/L(^j5)]]|'^. We have, 7 ’/L^[( 0 ,e')] = 
( 0 ,Rs^) - + diC). This implies that 0 = 

2^=1 Ci&xR and Re^ - cfi'^p + dfi‘ which again form 6 
linear equations with 6 unknowns. Solving this set of equa¬ 
tions yields c = 0 and d - ^e'. Hence the forth column of 
IT/L(^ is given by [ 0 , (.Re')^]^. We can apply the same 
procedure to obtain the fifth and sixth column as well. Combin¬ 
ing all of the columns together yields 


IT/L(^,-)]] 


eX 

e 


Re' Re^ Re^ O3 O3 O3 

PxRe* pxR^^ PxR^^ R^^ R^^ R^^ 


R 

PxR 


03x3 

R 
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